package com.rockwell.sniffyhunter.utils;

import android.graphics.PointF;

public class Vector2D implements Cloneable {
	public PointF coord;
	public int angleFromNorth;
	
	public Vector2D() {
		this(new PointF(0.0f, 0.0f), 0);
	}
	
	public Vector2D(PointF point, int angle) {
		coord = point;
		angleFromNorth = angle;
	}
	
	public Vector2D(int x, int y, int angle) {
		this(new PointF(x, y), angle);
	}
	
	public Vector2D(Vector2D rhs) {
		coord.x = rhs.coord.x;
		coord.y = rhs.coord.y;
		angleFromNorth = rhs.angleFromNorth;
	}
	
	public Vector2D clone() {
		Vector2D vect = new Vector2D();
		vect.angleFromNorth = angleFromNorth;
		vect.coord.x = coord.x;
		vect.coord.y = coord.y;
		return vect;
	}
	
	// Calculate the angle between the source vector and the destination vector
	public static int calculateAngleBetween(Vector2D currentVector, Vector2D destVector) {
		return destVector.angleFromNorth - currentVector.angleFromNorth;
	}
	
	// Calculate the Angle between a vector to its destination point.
	public static int calculateAngleTo(Vector2D currentPos, Vector2D destPos) {
		// Establish an arbitrary point along currVector
		PointF arbitraryPoint = new PointF();
		arbitraryPoint.x = (float) (currentPos.coord.x + Math.sin(Math.toRadians(currentPos.angleFromNorth)));
		arbitraryPoint.y = (float) (currentPos.coord.y + Math.cos(Math.toRadians(currentPos.angleFromNorth)));
		
		// Assume aVector to be from Point currentPos to arbitraryPoint
		// Calculate aVector and its aVectMagnitude
		PointF aVector = new PointF();
		aVector.x = arbitraryPoint.x - currentPos.coord.x;
		aVector.y = arbitraryPoint.y - currentPos.coord.y;
	
		double aVectMagnitude = Math.sqrt((aVector.x * aVector.x) + (aVector.y * aVector.y));
		
		// Assume bVector to be from Point currentPos to destPos
		// Calculate bVector and its bVectMagnitude
		PointF bVector = new PointF();
		bVector.x = destPos.coord.x - currentPos.coord.x;
		bVector.y = destPos.coord.y - currentPos.coord.y;
		
		double bVectMagnitude = Math.sqrt((bVector.x * bVector.x) + (bVector.y * bVector.y));

		// Calculate dot products of aVector and bVector
		double dotProduct = (aVector.x * bVector.x) + (aVector.y * bVector.y); 
		
		// Cosine Rule: a.b = |a|*|b|*cos(tetha)
		int offsetAngle = (int) Math.toDegrees(Math.acos(dotProduct/(aVectMagnitude * bVectMagnitude)));
		
		// Signed Angle by using cross products
		double crossProduct = (aVector.x * bVector.y) - (aVector.y * bVector.x);
		return (crossProduct > 0) ? -offsetAngle : offsetAngle;
	}
	
	// Calculate distance between 2 vectors
	public static double calculateDistanceBetween(Vector2D currentPos, Vector2D destPos) {
		PointF vector = new PointF();
		vector.x = Math.abs(destPos.coord.x - currentPos.coord.x);
		vector.y = Math.abs(destPos.coord.y - currentPos.coord.y);
		return Math.sqrt((vector.x * vector.x) + (vector.y * vector.y));
	}
}
